#ifndef MPC_EW2D_PatternedW_H
#define MPC_EW2D_PatternedW_H

#include "Controller.h"

// MPC1_PatternedW defined the following optimization problem
//
// min_{T,W_0,A}{J(T,W0,A)}
// where 
// >> J = q_r\times(r^2_f-r^2_sp)^2+q_m\times(m^2_f-m^2_sp)^2
//    r^2_f is the predicted value of r^2 at t = t_end
// >> r^2_f = eq_r2(T,W0,A,t_end-t_now,current_surface)
//    m^2_f is defined in a similar way
// >> m^2_f = eq_m2(T,W0,A,t_end-t_now,current_surface)
// S.T.:
//   T_min <= T <= T_max
//   W_min <= W <= W_max
//   \abs{T(t)-T(t-dt)}<= dT_max
//   \abs{W(t)-W(t_dt)}<= dW_max
//   0 < A < W
//
// SUMMARY
// Parameters: 
//  - From feedback: current_surface, t_end-t_now
//  - Tuning parameters: r^2_sp, m^2_sp, q_r, q_m
// Decision variables: T, W0, A
// Inequality constraints: 
// -\infty <= g(x) = A-W = x[2]-x[1] <= 0
// -- Upper/lower limits of variables --
// max{T_{min},T_old-dT_max} <= T <= min{T_{max},T_old+dT_max}
// max{W_{min},W_old_dW_max} <= W <= min{W_{max},W_old+dW_max}
// 0 <= A <= \infty

// -------------------------------------------
// C2 = C2(T,W)
class eq_C2:public MathFunction{
 private:
  // parameters
  int LatticeSize;
 public:
  explicit eq_C2(int i_LatticeSize);
  virtual ~eq_C2();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  virtual bool parameterize(const double *para);
  virtual bool parameterize(int para_ID,double para_val);
};
// ------------------------------------------
// sigma^2 = sigma^2(T,W)
class eq_sigma2:public MathFunction{
 private:
  int LatticeSize;
 public:
  explicit eq_sigma2(int i_LatticeSize);
  virtual ~eq_sigma2();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  virtual bool parameterize(const double *para);
  virtual bool parameterize(int para_ID, double para_val);
};
// ------------------------------------------
// cov_z_p = cov_z_p(T,W) = cov_z_p(C2(T,W),sigma2(T,W))
class eq_cov_z_p:public MathFunction{
 private:
  // parameters
  int p;
  int m;
  double dt;
  double cov_z_m;
  double Xmax;
  int    LatticeSize;
  // Auxillary variables
  int index;
  eq_C2 f_c2;
  eq_sigma2 f_sigma2;
 public:
  explicit eq_cov_z_p(int    i_p,
             int    i_m,
	     double i_Xmax,
	     int    i_LatticeSize,
	     double i_dt,
	     double i_cov_z_m);
  virtual ~eq_cov_z_p();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  virtual bool parameterize(int para_ID,double para_val);
};
// ----------------------------------------
// z2_p = z2_p(T,W,A) = z2_p(C2(T,W),sigma2(T,W),W_pmn(A))
class eq_z2_p:public MathFunction{
 private:
  // parameters
  int p;
  int m;
  double dt;
  double cov_z_m;
  double mean_z_m;
  double Freq;
  double Xmax;
  int    LatticeSize;
  // auxillary variables
  int index;
  eq_C2 f_c2;
  eq_sigma2 f_sigma2;
 public:
  explicit eq_z2_p(int    i_p,
          int    i_m,
	  double i_dt,
	  double i_cov_z_m,
	  double i_mean_z_m,
	  double i_Freq, 
	  double i_Xmax,
	  int    i_LatticeSize);
  virtual ~eq_z2_p();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  virtual bool parameterize(int para_ID,double para_val);
  virtual bool parameterize(const double* para);

  // Additional functions
  double eq_W_pm(double W,double A);
  double eq_dW_pm_dA();
};
// --------------------------------------------
// cov_z_p = cov_z_p2(C2,sigma2)
class eq_cov_z_p2:public MathFunction{
 private:
  // parameters
  int p;
  int m;
  double dt;
  double cov_z_m;
  // Constant
  static const double Xmax;
 public:
  explicit eq_cov_z_p2();
  virtual ~eq_cov_z_p2();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  virtual bool parameterize(int para_ID,double para_val);
};
// --------------------------------------------
// r2_f = r2_f(T,W,A)
class eq_r2_f:public MathFunction{
 private:
  // Parameters
  int mode;
  double Xmax;
  double Freq;
  int LatticeSize;
  double dt;
  double *cov_z_m[2];
  double *mean_z_m[2];
  
  // Auxillary variables
  eq_z2_p z2_p;
 public:
  explicit eq_r2_f(double i_dt,
          int    i_mode,
	  double i_Xmax,
	  int    i_LatticeSize,
	  double i_Freq);
  virtual ~eq_r2_f();
  virtual double value(const double *x);
  virtual void gradient(const double *x, double *grad);
  virtual bool parameterize(int para_ID, double para_val);
  virtual bool parameterize(int para_ID,double *para_val);
};
// ------------------------------------------
// m2_f = m2_f(W,A)
class eq_m2_f:public MathFunction{
 private:
  // Parameters
  int mode;
  double Xmax;
  double Freq;
  int LatticeSize;
  double dt;
  double *cov_z_m[2];
  double *mean_z_m[2];
  double *K[2];
  
  // Auxillary variables
  eq_z2_p z2_p;
 public:
  explicit eq_m2_f(double i_dt,
	  int    i_mode,
	  double i_Xmax,
	  int    i_LatticeSize,
	  double i_Freq);
  virtual ~eq_m2_f();
  virtual bool parameterize(int para_ID,double para_val);
  virtual bool parameterize(int para_ID,double* para_val);
  //  virtual bool parameterize(const double *para);
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
};
// ------------------------------------------
// f = f(W,A) = q_r*(r2_set-r2_f)^2
class eq_f:public MathFunction{
 private:
  double r2_set;
  double q_r;
  double m2_set;
  double q_m;
  int    mode;
  int    LatticeSize;
  double Xmax;
  double dt;
  double Freq;

  double *cov_z_m[2];
  double *mean_z_m[2];

  eq_r2_f f_r2_f;
  eq_m2_f f_m2_f;
 public:
  explicit eq_f(double i_r2_set,
       double i_q_r,
       double i_m2_set,
       double i_q_m,
       double i_dt, 
       int    i_mode,
       int    i_LatticeSize,
       double i_Xmax,
       double i_Freq);
  virtual ~eq_f();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  virtual bool parameterize(int para_ID,double para_val);
  virtual bool parameterize(int para_ID,double *para_val);
};
// -----------------------------------------
class MPC1_PatternedW : public MPC_Formulation{
  // parameters in the optimization problem
  // I/O parameters -----------------
  int num_decision_val;
  double T_old;
  double W_old;
  double A_old;
  double T;
  double W_ini;
  double A_ini;
  double Freq;

  // Cost function ------------------
  double r2_set,q_r; // Unit of r2: (nm)^2
  double m2_set,q_m; // Unit of m2: dimensionless
  eq_f   cost_fun;

  // Constraints -------------------
  double T_min, T_max; // Unit: K
  double W_min, W_max; // Unit: layer/s
  double A_min, A_max; // Unit: layer/s
  double dT_max;
  double dW_max;
  
  // Model parameter ---------------
  int    LatticeSize;
  double Xmax;       // length of the 2D surface domain, unit: nm
  double t_end;
  double C2;
  double sigam2;
  double time;
  double dt;          // dt = t_end-time;

  // Input specification parameters-
  int    mode;

  double *cov_z_m[2]; // measurement
  double *mean_z_m[2];
  double *cov_z_p[2]; // prediction
  double *mean_z_p[2];
  double *dz_square_dT[2];
  double *dz_square_dW[2];
  double *dz_square_dA[2];
 public:
  /** default constructor */
  explicit MPC1_PatternedW(double i_r2_set,
			   double i_q_r2,
			   double i_m2_set,
			   double i_q_m2,
			   int    i_LatticeSize,
			   double i_Xmax,
			   double i_t_end,
			   double i_dt,
			   int    i_mode,
			   double i_Freq);
  explicit MPC1_PatternedW(string option_file);
  /** default destructor */
  ~MPC1_PatternedW();
  
  // Virtual function from MPC_Formulation
  virtual void reset();
  virtual void parameterize(double time,double** Inputs);
  virtual void get_solution(double* x);
  virtual void setPara(int para_id,double para_val);

  /**@name Overloaded from TNLP */
  //@{
  /** Method to return some info about the nlp */
  virtual bool get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
                            Index& nnz_h_lag, IndexStyleEnum& index_style);

  /** Method to return the bounds for my problem */
  virtual bool get_bounds_info(Index n, Number* x_l, Number* x_u,
                               Index m, Number* g_l, Number* g_u);

  /** Method to return the starting point for the algorithm */
  virtual bool get_starting_point(Index n, bool init_x, Number* x,
                                  bool init_z, Number* z_L, Number* z_U,
                                  Index m, bool init_lambda,
                                  Number* lambda);

  /** Method to return the objective value */
  virtual bool eval_f(Index n, const Number* x, bool new_x, Number& obj_value);

  /** Method to return the gradient of the objective */
  virtual bool eval_grad_f(Index n, const Number* x, bool new_x, Number* grad_f);

  /** Method to return the constraint residuals */
  virtual bool eval_g(Index n, const Number* x, bool new_x, Index m, Number* g);

  /** Method to return:
   *   1) The structure of the jacobian (if "values" is NULL)
   *   2) The values of the jacobian (if "values" is not NULL)
   */
  virtual bool eval_jac_g(Index n, const Number* x, bool new_x,
                          Index m, Index nele_jac, Index* iRow, Index *jCol,
                          Number* values);

  /** Method to return:
   *   1) The structure of the hessian of the lagrangian (if "values" is NULL)
   *   2) The values of the hessian of the lagrangian (if "values" is not NULL)
   */
  virtual bool eval_h(Index n, const Number* x, bool new_x,
                      Number obj_factor, Index m, const Number* lambda,
                      bool new_lambda, Index nele_hess, Index* iRow,
                      Index* jCol, Number* values);

  //@}

  /** @name Solution Methods */
  //@{
  /** This method is called when the algorithm is complete so the TNLP can store/write the solution */
  virtual void finalize_solution(SolverReturn status,
                                 Index n, const Number* x, const Number* z_L, const Number* z_U,
                                 Index m, const Number* g, const Number* lambda,
                                 Number obj_value,
				 const IpoptData* ip_data,
				 IpoptCalculatedQuantities* ip_cq);
  //@}

private:
  /**@name Methods to block default compiler methods.
   * The compiler automatically generates the following three methods.
   *  Since the default compiler implementation is generally not what
   *  you want (for all but the most simple classes), we usually 
   *  put the declarations of these methods in the private section
   *  and never implement them. This prevents the compiler from
   *  implementing an incorrect "default" behavior without us
   *  knowing. (See Scott Meyers book, "Effective C++")
   *  
   */
  //@{
  //  MPC_Formulation();
  MPC1_PatternedW(const MPC_Formulation&);
  MPC1_PatternedW& operator=(const MPC_Formulation&);
  //@}
  inline int    index1D(int m){return m;};
};

#endif
